#include "AssociationSet.h"
#include <map>

bool operator< (const AssociationSet& a, const AssociationSet &b)
{
	return a.val > b.val;	
}

bool operator> (const AssociationSet& a, const AssociationSet &b)
{
	return a.val < b.val;	
}

AssociationSet::AssociationSet(){
	val = 0;
	len = 0;
	selected_associations = new vector<Association *>();
	possible_associations = new vector<Association *>();
}

AssociationSet::~AssociationSet(){
	delete selected_associations;
	delete possible_associations;
	delete hash;
}

char * AssociationSet::getHashValue(){
	return hash;
};

AssociationSet * AssociationSet::clone(){

	AssociationSet * ret = new AssociationSet();
	ret->val = val;
	ret->len = len;
	ret->hash = new char [1+len/8];
	for(int i = 0; i < 1+len/8; i++){ret->hash[i] = hash[i];}

	for(unsigned int i = 0; i < selected_associations->size(); i++)
	{
		ret->selected_associations->push_back(selected_associations->at(i));
	}
	for(unsigned int i = 0; i < possible_associations->size(); i++)
	{
		ret->possible_associations->push_back(possible_associations->at(i));
	}
	return ret;
};

float ** AssociationSet::jc(){
	float ** mat = new float*[selected_associations->size()];
	for(unsigned int i = 0; i < selected_associations->size(); i++)
	{
		mat[i] = new float[selected_associations->size()];
	}
	for(unsigned int i = 0; i < selected_associations->size(); i++)
	{
		mat[i][i] = 0;
		for(unsigned int j = i+1; j < selected_associations->size(); j++)
		{
			Association * t1 = selected_associations->at(i);
			Association * t2 = selected_associations->at(j);
			mat[i][j] = t1->compatable(t2);
			mat[j][i] = mat[i][j];
		}
	}
	return mat;
};

bool AssociationSet::select_association(int index){
	Association * t = possible_associations->at(index);
	possible_associations->at(index) = possible_associations->back();
	possible_associations->pop_back();
	bool acceptable = true;
	for(unsigned int i = 0; i < selected_associations->size(); i++)
	{
		Association * t2 = selected_associations->at(i);
		if(!t->compatable(t2)){
			acceptable = false;
			break;
		}
	}
	if(acceptable){
		//printf("acceptable\n");
		selected_associations->push_back(t);
	}
	return acceptable;
};

void AssociationSet::update_possible(){
	printf("update_possible()\n");
	for(unsigned int i = 0; i < possible_associations->size(); i++)
	{
		printf("update_possible(): %i\n",i);
		Association * t1 = possible_associations->at(i);
		bool acceptable = true;
		for(unsigned int j = 0; j < selected_associations->size(); j++)
		{
			printf("update_possible(): %i,%i\n",i,j);
			Association * t2 = selected_associations->at(j);
			if(!t1->compatable(t2)){
				acceptable = false;
				break;
			}
		}
		if(!acceptable){
			possible_associations->at(i) = possible_associations->back();
			possible_associations->pop_back();
		}
	}
};

vector<AssociationSet * > * AssociationSet::explore(){
	vector<AssociationSet * > * asoSet = new vector<AssociationSet * >();
	
	for(unsigned int i = 0; i < possible_associations->size(); i++)
	{
		AssociationSet * tmp = clone();
		Association * t = tmp->possible_associations->at(i);
		int current_id = t->id; 
		tmp->hash[current_id / 8] += 1 << (current_id % 8);
	
		tmp->possible_associations->at(i) = tmp->possible_associations->back();
		tmp->possible_associations->pop_back();
		tmp->selected_associations->push_back(t);
		tmp->val 		= - 1.0f*tmp->estimateSearchValue();
		tmp->bestcase 	= - tmp->best_p() + tmp->val;
		asoSet->push_back(tmp);
		
		//bool ok = tmp->select_association(i);
		//if(ok){asoSet->push_back(tmp);}
		//else{delete tmp;}
	}
	
	return asoSet;
};

bool src_compare(Association * a,Association * b)
{
	if(a->type == b->type){
		if(a->type == keypoint_association){
			KeyPointAssociation * akp = (KeyPointAssociation *)a;
			KeyPointAssociation * bkp = (KeyPointAssociation *)b;
			if(akp->p1 == bkp->p1)	{return akp->weight > bkp->weight;}
			else					{return akp->p1 < bkp->p1;}
		}else if(a->type == plane_association){
			PlaneAssociation * ap = (PlaneAssociation *)a;
			PlaneAssociation * bp = (PlaneAssociation *)b;
			if(ap->p1 == bp->p1)	{return ap->weight 	> bp->weight;}
			else					{return ap->p1 		< bp->p1;}
		}else{return true;}
	}else{
		return a->type < b->type;
	}
}

bool dst_compare(Association * a,Association * b)
{
	if(a->type == b->type){
		if(a->type == keypoint_association){
			KeyPointAssociation * akp = (KeyPointAssociation *)a;
			KeyPointAssociation * bkp = (KeyPointAssociation *)b;
			if(akp->p2 == bkp->p2)	{return akp->weight > bkp->weight;}
			else					{return akp->p2 < bkp->p2;}
		}else if(a->type == plane_association){
			PlaneAssociation * ap = (PlaneAssociation *)a;
			PlaneAssociation * bp = (PlaneAssociation *)b;
			if(ap->p2 == bp->p2)	{return ap->weight > bp->weight;}
			else					{return ap->p2 < bp->p2;}
		}else{return true;}
	}else{
		return a->type < b->type;
	}
}

float AssociationSet::best_p(){
	vector<Association *> a;
	for(unsigned int i = 0; i < possible_associations->size() ; i++)
	{
		a.push_back(possible_associations->at(i));
	}
	sort(a.begin(), a.end(), src_compare);
	KeyPoint * last = 0;
	float sum_a = 0;
	for(unsigned int i = 0; i < a.size() ; i++)
	{
		Association * t = a.at(i);

		if(t->type == keypoint_association){
			KeyPointAssociation * tkp = (KeyPointAssociation *)t;
			KeyPoint * current = tkp->p1;
			float weight = tkp->weight;
			if(current != last){sum_a += weight;}
			last = current;
		}
	}
	sort(a.begin(), a.end(), dst_compare);
	last = 0;
	float sum_b = 0;
	for(unsigned int i = 0; i < a.size() ; i++)
	{
		Association * t = a.at(i);
		if(t->type == keypoint_association){
			KeyPointAssociation * tkp = (KeyPointAssociation *)t;
			if(tkp->p2 != last){sum_b += tkp->weight;}
			last = tkp->p2;
		}
	}
	if(sum_a > sum_b)	{return sum_b;}
	else				{return sum_a;}
}

double AssociationSet::estimateSearchValue(){
	float sum = 0;
	for(unsigned int i = 0; i < selected_associations->size(); i++){sum += selected_associations->at(i)->weight;}
	float kp_sum = 1;
	float pl_sum = 1;
	float combo_sum = 1;
	for(unsigned int i = 0; i < selected_associations->size(); i++){
		Association * ti = selected_associations->at(i);
		if(ti->type == keypoint_association){kp_sum += ti->weight;}
		if(ti->type == plane_association)	{pl_sum += ti->weight;}
		float combo_iter = 0;
		for(unsigned int j = i+1; j < selected_associations->size(); j++){
			Association * tj = selected_associations->at(j);
			float combo_weight = ti->weight * tj->weight;
			if(ti->type == keypoint_association && tj->type == keypoint_association){
				KeyPointAssociation * a = (KeyPointAssociation *)ti;
				KeyPointAssociation * b = (KeyPointAssociation *)tj;
				combo_iter += a->p1->point->distance(b->p1->point) * combo_weight;
			}
			if(ti->type == plane_association && tj->type == plane_association){
				//PlaneAssociation * a = (PlaneAssociation *)ti;
				//PlaneAssociation * b = (PlaneAssociation *)tj;
				//combo_iter += fabs(a->p1->angle(b->p1)) * combo_weight;
			}
		}
		combo_sum+=combo_iter/float(selected_associations->size()-i);
	}
	
	return kp_sum*pl_sum*combo_sum;
};

void AssociationSet::add(Association * a){
	selected_associations->push_back(a);
};

AssociationSet * AssociationSet::merge(AssociationSet * a){
	AssociationSet * ret = new AssociationSet();
	for(unsigned int i = 0; i < selected_associations->size(); i++){
		ret->selected_associations->push_back(selected_associations->at(i));
	}
	for(unsigned int i = 0; i < a->selected_associations->size(); i++){
		ret->selected_associations->push_back(a->selected_associations->at(i));
	}
	return ret;
};

int AssociationSet::mergeable(AssociationSet * set){
	for(unsigned int i = 0; i < selected_associations->size(); i++){
		Association * asso1 = selected_associations->at(i);
		for(unsigned int j = 0; j < set->selected_associations->size(); j++){
			Association * asso2 = set->selected_associations->at(j);
			int comp = asso1->compatable(asso2);
			if(comp != 2){return comp;}
		}
	}
	return 2;
};

